
/****************************************************************************/
/*Copyright (c) 2011, Florent DEVILLE.                                      */
/*All rights reserved.                                                      */
/*                                                                          */
/*Redistribution and use in source and binary forms, with or without        */
/*modification, are permitted provided that the following conditions        */
/*are met:                                                                  */
/*                                                                          */
/* - Redistributions of source code must retain the above copyright         */
/*notice, this list of conditions and the following disclaimer.             */
/* - Redistributions in binary form must reproduce the above                */
/*copyright notice, this list of conditions and the following               */
/*disclaimer in the documentation and/or other materials provided           */
/*with the distribution.                                                    */
/* - The names of its contributors cannot be used to endorse or promote     */
/*products derived from this software without specific prior written        */
/*permission.                                                               */
/* - The source code cannot be used for commercial purposes without         */ 
/*its contributors' permission.                                             */
/*                                                                          */
/*THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       */
/*"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT         */
/*LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS         */
/*FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE            */
/*COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,       */
/*INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,      */
/*BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;          */
/*LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER          */
/*CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT        */
/*LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN         */
/*ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           */
/*POSSIBILITY OF SUCH DAMAGE.                                               */
/****************************************************************************/

#include "RTMatrix44.h"


#include <iostream>
using std::cout;
using std::endl;

#include <math.h>
#include "RTVector3f.h"

namespace RT
{

	/*Default constructor*/
	RTMatrix44::RTMatrix44()
	{
		for(I32 i = 0; i < 4; i++)
			for(I32 j = 0; j < 4; j++)
				m_array[i][j] = 0;

	}

	/*Destructor*/
	RTMatrix44::~RTMatrix44(){}

	/*Set the matrix to the identity*/
	void RTMatrix44::identity()
	{
		for(I32 i = 0; i < 4; i++)
			for(I32 j = 0; j < 4; j++)
			{
				if( i == j)
					m_array[i][j] = 1;
				else
					m_array[i][j] = 0;
			}
	}

	/*Calculate the inverse matrix*/
	RTMatrix44 RTMatrix44::inverse() const
	{
		//the inverse set to the identity
		RTMatrix44 I;
		I.identity();

		//the entry matrix
		RTMatrix44 A;
		A = *this;
	
		//iterate through each line
		for(I32 k = 0; k < 4; k++)
		{
			//inverse line if a[k][k] = 0;
			if(A.m_array[k][k] == 0)
			{
				I32 i = 0;
				bool over = false;
				while(i < 4 && !over)
				{
					if(i == k)
					{
						i++;
						continue;
					}

					if(A.m_array[i][k] != 0)
					{
						over = true;

						//inverse row i and k
						for(I32 h = 0; h < 4; h++)
						{
							F32 temp = A.m_array[i][h];
							A.m_array[i][h] = A.m_array[k][h];
							A.m_array[k][h] = temp;

							temp = I.m_array[i][h];
							I.m_array[i][h] = I.m_array[k][h];
							I.m_array[k][h] = temp;
						}
					}
					i++;
				}
				//not inversible
				if(!over)
					return I;
			}

			//simplify the line to put a 1 in a[k][k]
			if(A.m_array[k][k] != 1)
			{
				F32 coeff = 1.f/A.m_array[k][k];
				for(I32 h = 0; h < 4; h++)
				{
					A.m_array[k][h] *= coeff;
					I.m_array[k][h] *= coeff;
				}
			}

			//pivot
			for(I32 i = 0; i < 4; i++)
			{
				if(i == k)
					continue;

				F32 coeff = A.m_array[i][k];

				for(I32 h = 0; h < 4; h++)
				{
					A.m_array[i][h] = A.m_array[i][h] - coeff * A.m_array[k][h];
					I.m_array[i][h] = I.m_array[i][h] - coeff * I.m_array[k][h];
				}
			}
		}

		return I;
	}

	/*Display in std out*/
	void RTMatrix44::display()const
	{
		for(I32 i = 0; i < 4; i++)
		{
			for(I32 j = 0; j < 4; j++) 
				cout<<m_array[i][j]<<" ";

			cout<<endl;
		}			
	}

	/*addition*/
	RTMatrix44 RTMatrix44::operator+(const RTMatrix44& m)const
	{
		RTMatrix44 sum;
		for(I32 i = 0; i < 4; i++)
			for(I32 j = 0; j < 4; j++)
				sum.m_array[i][j] = m_array[i][j] + m.m_array[i][j];

		return sum;
	}

	/*substraction*/
	RTMatrix44 RTMatrix44::operator-(const RTMatrix44& m)const
	{
		RTMatrix44 res;
		for(I32 i = 0; i < 4; i++)
			for(I32 j = 0; j < 4; j++)
				res.m_array[i][j] = m_array[i][j] - m.m_array[i][j];

		return res;
	}

	/*multiplication*/
	RTMatrix44 RTMatrix44::operator*(const RTMatrix44& m)const
	{
		RTMatrix44 res;

		for(I32 i = 0; i < 4; i++) //row
		{
			for(I32 j = 0; j < 4; j++) //column
			{
				res.m_array[i][j] = 0;
				for(I32 h = 0; h < 4; h++) //offset
					res.m_array[i][j] += m_array[i][h] * m.m_array[h][j];
			}
		}

		return res;
	}

	/*Multiplication with a vector*/
	RTVector3f RTMatrix44::operator*(const RTVector3f& v)const
	{
		RTVector3f res;
		res.x = m_array[0][0] * v.x + m_array[0][1] * v.y + m_array[0][2] * v.z + m_array[0][3];
		res.y = m_array[1][0] * v.x + m_array[1][1] * v.y + m_array[1][2] * v.z + m_array[1][3];
		res.z = m_array[2][0] * v.x + m_array[2][1] * v.y + m_array[2][2] * v.z + m_array[2][3];

		return res;
	}

	/*add and assign*/
	void RTMatrix44::operator+=(const RTMatrix44& m)
	{
		for(I32 i = 0; i < 4; i++)
			for(I32 j = 0; j < 4; j++)
				m_array[i][j] = m_array[i][j] + m.m_array[i][j];
	}

	/*sub and assign*/
	void RTMatrix44::operator-=(const RTMatrix44& m)
	{
		for(I32 i = 0; i < 4; i++)
			for(I32 j = 0; j < 4; j++)
				m_array[i][j] = m_array[i][j] - m.m_array[i][j];
	}

	/*assign*/
	void RTMatrix44::operator=(const RTMatrix44& m)const
	{
		//copy array to array
		memcpy((void*)m_array, m.m_array, sizeof(F32)*16);
	}

	/*create a translation matrix*/
	void RTMatrix44::createTranslation(F32 x, F32 y , F32 z)
	{
		identity();
		m_array[3][0] = x;
		m_array[3][1] = y;
		m_array[3][2] = z;
	}

	/*Create a translation matrix from a vector*/
	void RTMatrix44::createTranslation(const RTVector3f& v)
	{
		identity();
		m_array[3][0] = v.x;
		m_array[3][1] = v.y;
		m_array[3][2] = v.z;
	}

	/*create a rotation matrix around X axis*/
	void RTMatrix44::createRotationX(F32 radiusAngle)
	{
		identity();
		F32 c = cos(radiusAngle);
		F32 s = sin(radiusAngle);
		m_array[1][1] = c;
		m_array[1][2] = s;
		m_array[2][1] = -s;
		m_array[2][2] = c;
	}

	/*create a rotation matrix around Y axis*/
	void RTMatrix44::createRotationY(F32 radiusAngle)
	{
		identity();
		F32 c = cos(radiusAngle);
		F32 s = sin(radiusAngle);
		m_array[0][0] = c;
		m_array[0][2] = -s;
		m_array[2][0] = s;
		m_array[2][2] = c;
	}

	/*create a rotation matrix around Z axis*/
	void RTMatrix44::createRotationZ(F32 radiusAngle)
	{
		identity();
		F32 c = cos(radiusAngle);
		F32 s = sin(radiusAngle);
		m_array[0][0] = c;
		m_array[0][1] = s;
		m_array[1][0] = -s;
		m_array[1][1] = c;
	}

	/*create a rotation matrix a specific axis*/
	void RTMatrix44::createRotation(const RTVector3f& axis, F32 angle)
	{
		F32 c = cos(angle);
		F32 s = sin(angle);

		F32 XPowerTwo = axis.x * axis.x;
		F32 YPowerTwo = axis.y * axis.y;
		F32 ZPowerTwo = axis.z * axis.z;

		m_array[0][0] = XPowerTwo + (1 - XPowerTwo) * c;
		m_array[0][1] = axis.x * axis.y * (1 - c) - axis.z * s;
		m_array[0][2] = axis.x * axis.z * (1 - c) + axis.y * s;
		m_array[0][3] = 0;
		m_array[1][0] = axis.x * axis.y * (1 - c) + axis.z * s;
		m_array[1][1] = YPowerTwo + (1 - YPowerTwo) * c;
		m_array[1][2] = axis.y * axis.z * (1 - c) - axis.x * s;
		m_array[1][3] = 0;
		m_array[2][0] = axis.x * axis.z * (1 - c) - axis.y * s;
		m_array[2][1] = axis.y * axis.z * (1 - c) + axis.x * s;
		m_array[2][2] = ZPowerTwo + (1 - ZPowerTwo) * c;
		m_array[2][3] = m_array[3][0] = m_array[3][1] = m_array[3][2] = 0;
		m_array[3][3] = 1;

	}

	/*create a non uniform scale matrix*/
	void RTMatrix44::createScale(const RTVector3f& s)
	{
		identity();

		m_array[0][0] = s.x;
		m_array[1][1] = s.y;
		m_array[2][2] = s.z;
	}

	/*create a uniform scale matrix*/
	void RTMatrix44::createScale(F32 s)
	{
		identity();

		m_array[0][0] = s;
		m_array[1][1] = s;
		m_array[2][2] = s;
	}

	/*fill the matrix using opengl model of matrix*/
	void RTMatrix44::getOpenGLMatrix(F64* matrix)
	{
		matrix[0] = m_array[0][0];
		matrix[1] = m_array[0][1];
		matrix[2] = m_array[0][2];
		matrix[3] = m_array[0][3];

		matrix[4] = m_array[1][0];
		matrix[5] = m_array[1][1];
		matrix[6] = m_array[1][2];
		matrix[7] = m_array[1][3];

		matrix[8] = m_array[2][0];
		matrix[9] = m_array[2][1];
		matrix[10] = m_array[2][2];
		matrix[11] = m_array[2][3];

		matrix[12] = m_array[3][0];
		matrix[13] = m_array[3][1];
		matrix[14] = m_array[3][2];
		matrix[15] = m_array[3][3];

	}

	//calculate frenet matrix
	void RTMatrix44::createFrenet(const RTVector3f& position, const RTVector3f& direction, const RTVector3f& up)
	{

		RTVector3f normal = direction.cross(up);
		normal = normal * -1;
		normal.normalize();

		RTVector3f binormal = direction.cross(normal);
		//binormal = binormal * -1;
		binormal.normalize();

		m_array[0][0] = normal.x;
		m_array[0][1] = normal.y;
		m_array[0][2] = normal.z;

		m_array[1][0] = binormal.x;
		m_array[1][1] = binormal.y;
		m_array[1][2] = binormal.z;

		m_array[2][0] = direction.x;
		m_array[2][1] = direction.y;
		m_array[2][2] = direction.z;

		m_array[3][0] = position.x;
		m_array[3][1] = position.y;
		m_array[3][2] = position.z;

		m_array[0][3] = m_array[1][3] = m_array[2][3] = 0;
		m_array[3][3] = 1;
	}

	//build a matrix using three vectors as columns
	void RTMatrix44::buildColumn(const RTVector3f& c1, const RTVector3f& c2, const RTVector3f& c3)
	{
		m_array[0][0] = c1.x;
		m_array[1][0] = c1.y;
		m_array[2][0] = c1.z;
		m_array[3][0] = 0;

		m_array[0][1] = c2.x;
		m_array[1][1] = c2.y;
		m_array[2][1] = c2.z;
		m_array[3][1] = 0;

		m_array[0][2] = c3.x;
		m_array[1][2] = c3.y;
		m_array[2][2] = c3.z;
		m_array[3][2] = 0;

		m_array[0][3] = 0;
		m_array[1][3] = 0;
		m_array[2][3] = 0;
		m_array[3][3] = 1;
	}

	//calculate the determinant
	F32 RTMatrix44::det()const
	{
		F32 detM0 =	m_array[1][1]*m_array[2][2]*m_array[3][3] + 
						m_array[1][2]*m_array[2][3]*m_array[3][1] +
						m_array[1][3]*m_array[2][1]*m_array[3][2] -
						m_array[1][3]*m_array[2][2]*m_array[3][1] -
						m_array[2][3]*m_array[3][2]*m_array[1][1] -
						m_array[3][3]*m_array[1][2]*m_array[2][1];

		F32 detM1 =	m_array[1][0]*m_array[2][2]*m_array[3][3] + 
						m_array[2][0]*m_array[3][1]*m_array[1][3] +
						m_array[1][2]*m_array[2][3]*m_array[3][0] -
						m_array[1][3]*m_array[2][2]*m_array[3][0] -
						m_array[2][3]*m_array[3][1]*m_array[1][0] -
						m_array[3][3]*m_array[1][2]*m_array[2][0];

		F32 detM2 =	m_array[1][0]*m_array[2][1]*m_array[3][3] + 
						m_array[2][0]*m_array[3][1]*m_array[1][3] +
						m_array[1][1]*m_array[2][3]*m_array[3][0] -
						m_array[1][3]*m_array[2][1]*m_array[3][0] -
						m_array[2][3]*m_array[3][1]*m_array[1][0] -
						m_array[3][3]*m_array[1][1]*m_array[2][0];

		F32 detM3 =	m_array[1][0]*m_array[2][1]*m_array[3][2] + 
						m_array[2][0]*m_array[3][1]*m_array[1][2] +
						m_array[1][1]*m_array[2][2]*m_array[3][0] -
						m_array[1][2]*m_array[2][1]*m_array[3][0] -
						m_array[2][2]*m_array[3][1]*m_array[1][0] -
						m_array[3][2]*m_array[1][1]*m_array[2][0];

		return m_array[0][0]*detM0 - m_array[0][1]*detM1 + m_array[0][2]*detM2 - m_array[0][3]*detM3;
	}

}